//#include <bits/stl_alloc.h>
#include <vector>

# define DEBUG_LEVEL 9
#include "debug_func.h"
#include "blast_results.h"
#include "matrix.h"

//
// This piece of code is a test program that uses the algorithms in 
// btl_sequence_algorithms.h, currently only a Pairwise Sequence Alignment. 
//
// Copyright (C) 1998,1999 Birkbeck College, Malet Street, London WC1E 7HX, U.K.
// d.moss@mail.cryst.bbk.ac.uk or m.williams@biochemistry.ucl.ac.uk
// 
// This library is free software; you can redistribute it and/or modify it
// under the terms of the GNU Library General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at your
// option) any later version.  This library is distributed in the hope
// that it will be useful, but WITHOUT ANY WARRANTY; without even the
// implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the GNU Library General Public License for more details.
// You should have received a copy of the GNU Library General Public
// License along with this library; if not, write to the Free Software
// Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
///////////////////////////////////////////////////////////////////////////
//
// Author: Breen Sweeney, Will Pitt, Mark Williams, Leo Goodstadt                
// 
///////////////////////////////////////////////////////////////////////////
// Modification History:
// Original version: B.S. (assistance from W.P)             Date: 30/08/98
// modified by: M.A.W.                                      Date: 31/08/99
// Last modified by: LG                                     Date: 20/04/2006
// Comments:   
///////////////////////////////////////////////////////////////////////////
// 
// Brief Description of Code:
// 
// Carry out alignment of two sequences entered from keyboard or from a PIR 
// format file. User selectable gap penalty. With the supplied test.pir file
// this reproduces the results in fig. 2 of the original Needleman & Wunsch 
// (1970) J. Mol. Biol. 48, 443-453 publication
//
// For further details see the documentation for the matrix, vector classes 
// and the algorithms header files.
///////////////////////////////////////////////////////////////////////////////
// 


#include <btl/btl_matrix.h>



template <class InputIterator>
void my_needleman_wunsch_alignment(	InputIterator first1, InputIterator last1,
                                    InputIterator first2, InputIterator last2,
                                    std::vector<t_gap>& pos1,
                                    std::vector<t_gap>& pos2, 
                                    double match_score = 1.0, double mismatch_score = 0.0,
                                    double gap_penalty = 1.0, double gap_length_multiplier = 0.0)
{

	// get the lengths of the sequences + 1
	unsigned length_s = (last1-first1);
	unsigned length_t = (last2-first2);

	// Create a score matrix
	btl::matrix<float>  matrix_s(length_s + 1,length_t + 1);

	// Calculate matrix values for the last row and column.

	// The scoring matrix is padded at the end by a blank

	matrix_s[length_s][length_t] = 0.0;

	for (unsigned i = 0; i < length_s ; ++i)
	{
		matrix_s[i][length_t] = (i-length_s+2)*gap_length_multiplier - gap_penalty;
	}
	for (unsigned j = 0; j < length_t ; ++j)
	{
		matrix_s[length_s][j] = (j-length_t+2)*gap_length_multiplier - gap_penalty;
	}


	// Calculate the other values in the matrix.
	for (int i = length_s - 1; i>-1; i--)
	{
		for (int j = length_t - 1; j>-1; j--)
		{
			// We now calculate the maximum of the row cross-ways and to the right, 
			// and the column cross-ways and down.

			// Get the cross-value.
			double cval = matrix_s[i+1][j+1];

			// Get the down-value.
			double dval = matrix_s[i+1][j+1] - gap_length_multiplier; 
			for (unsigned ii=i+2; ii<length_s; ii++)
			{
				if ((matrix_s[ii][j+1] - (ii-i-1)*gap_length_multiplier) > dval)
					dval = matrix_s[ii][j+1] - (ii-i-1)*gap_length_multiplier;
			}


			// Get the right-value       
			double rval = matrix_s[i+1][j+1] - gap_length_multiplier;   
			for (unsigned jj = j + 2; jj <= length_t; jj++)
			{
				if ((matrix_s[i+1][jj] - (jj-j-1)*gap_length_multiplier) > rval)
					rval = matrix_s[i+1][jj] - (jj-j-1)*gap_length_multiplier;
			}

			double tmp_max = std::max(dval,rval);
			tmp_max = std::max(tmp_max - gap_penalty, cval);
			if (*(first1+i) == *(first2+j))
				matrix_s[i][j] = tmp_max + match_score;
			else
				matrix_s[i][j] = tmp_max + mismatch_score;      

		}
	}    


	// Trace path back through the matrix: choosing successive maxima of the 
	// right, down and diagonal elements
	// In event of tie choose diagonal in preference to right or down
	// and down in preference to right i.e. choose shortest path through matrix 
	// and add gaps to the sequence in preference to the target. 
	// This choice mechanism is somewhat arbitrary from a theoretical standpoint 
	// yet seems sensible from a biological one. There may be alternative start points
	// where scores of two alignments match here we choose such that the target
	// has minimum gaps


	// Find the maximum score in the first row or column

	unsigned j_index=0;
	double max_score = matrix_s[0][0]; 
	for (unsigned j = 0; j <= length_t; j++)
	{
		if (matrix_s[0][j] >= max_score)
		{
			max_score = matrix_s[0][j];
			j_index = j;
		}
	}
	unsigned i_index=0; 
	for (unsigned i=0; i<length_s; i++)
	{
		if (matrix_s[i][0] >= max_score)
		{
			max_score = matrix_s[i][0];
			i_index = i;
			j_index = 0;
		}
	}

	// the starting sequence element is (i_index,j_index)

	unsigned i = 0U, j = 0U;


	if (i_index == 0)
	{
		j=j_index;
		pos1.push_back(t_gap(0,  j_index));
	}
	else
	{
		i=i_index;   
		pos2.push_back(t_gap(0,  i_index));
	}

	unsigned gap_len1 = 0, gap_len2 = 0;
	unsigned gap_start1 = 0, gap_start2 = 0;

	while (i < length_s && j < length_t)
	{
		if ((matrix_s[i+1][j+1] >= matrix_s[i][j+1]) 
			&& (matrix_s[i+1][j+1] >= matrix_s[i+1][j]))
		{
			if (gap_len1)
			{
				pos1.push_back(t_gap(gap_start1, gap_len1));
				gap_start1 = gap_len1 = 0;
			}
			else if (gap_len2)
			{
				pos2.push_back(t_gap(gap_start2, gap_len2));
				gap_start2 = gap_len2 = 0;
			}
			++i;
			++j;
		}
		else
		{
			if (matrix_s[i][j+1] > matrix_s[i+1][j])	   // right > down
			{
				if (!gap_len1)
					gap_start1 = i + 1;
				++gap_len1;
				++j;

			}
			else
			{
				if (!gap_len2)
					gap_start2 = j + 1;
				++gap_len2;
				++i;
			}
		}
	}
	if (gap_len1)
	{
		pos1.push_back(t_gap(gap_start1, gap_len1));
		gap_start1 = gap_len1 = 0;
	}
	else if (gap_len2)
	{
		pos2.push_back(t_gap(gap_start2, gap_len2));
		gap_start2 = gap_len2 = 0;
	}

	// pad out the ends of sequences where necessary     

	if (i != length_s)
		pos2.push_back(t_gap(length_t, length_s - i));
	if (j != length_t)
		pos1.push_back(t_gap(length_s, length_t - j));
	
/*	//dump matrix
	std::cerr << "\t";
	for (unsigned i = 0; i < length_s; ++i)
		std::cerr << "\t" << i;
	std::cerr << "\n";
	std::cerr << "\t";
	for (unsigned i = 0; i < length_s; ++i)
		std::cerr << "\t" << *(first1 + i);
	std::cerr << "\n";
	for (unsigned j = 0; j <= length_t; ++j)
	{
		std::cerr << j;
		if (j < length_t)
			std::cerr << "\t" << *(first2 + j);
		else
			std::cerr << "\t ";
		for (unsigned i = 0; i <= length_s; ++i)
		{
			std::cerr << "\t" << matrix_s[i][j];
		}
		std::cerr << "\n";
	}
	*/
}


struct t_identity
{
	const float match_val;
	const float mismatch_val;
	t_identity(float f, float m) : match_val(f), mismatch_val(m){}
	float operator()(char c1, char c2){return (c1 == c2 ? match_val : mismatch_val);}
};

template <class InputIterator>
void my_needleman_wunsch_alignment2(InputIterator first1, InputIterator last1,
									InputIterator first2, InputIterator last2,
									std::vector<t_gap>& pos1,
									std::vector<t_gap>& pos2, 
									float match_score = 1.0, 
									float mismatch_score = 0.0,
									float gap_penalty = 1.0, 
									float gap_extension = 0.0)
{
	return
		my_needleman_wunsch_alignment2(first1, last1,
									first2, last2,
									pos1,
									pos2, 
									gap_penalty, 
									gap_extension,
									t_identity(match_score, mismatch_score));
}

template <class InputIterator, typename FUNC>
void my_needleman_wunsch_alignment2(InputIterator first1, InputIterator last1,
									InputIterator first2, InputIterator last2,
									std::vector<t_gap>& pos1,
									std::vector<t_gap>& pos2, 
									float gap_penalty = 1.0, 
									float gap_extension = 0.0,
									FUNC match
									)
{

	// get the lengths of the sequences + 1
	unsigned len1 = (last1-first1);
	unsigned len2 = (last2-first2);

	// Create a score matrix
	btl::matrix<float>  matrix_gap(len1 + 1, len2 + 1);
	btl::matrix<float>  matrix(len1 + 1, len2 + 1);

	// Calculate matrix values for the first row and column.

	// last column and row
	for (unsigned i = 0; i <= len1 ; ++i)
		matrix[i][len2] = - gap_penalty - (len1 - i - 2) * gap_extension;
	for (unsigned i = 0; i <= len2 ; ++i)
		matrix[len1][i] = - gap_penalty - (len2 - i - 2) * gap_extension;

	matrix[len1][len2] = 0.0F;

	for (unsigned i = 0; i <= len1 ; ++i)
		matrix_gap[i][len2] = 0;
	for (unsigned i = 0; i <= len2 ; ++i)
		matrix_gap[len1][i] = 0;

	
	// Calculate the other values in the matrix.
	{
		for (unsigned ii = len1; ii > 0; --ii)
		{
			for (unsigned jj = len2; jj > 0; --jj)
			{
				matrix[ii - 1][jj - 1] =	match(*(first1 + (ii - 1)), 
												  *(first2 + (jj - 1))) +
											std::max(matrix[ii][jj], 
													 matrix_gap[ii][jj]);
	
				float gap_open = std::max(matrix[ii][jj - 1], matrix[ii - 1][jj]) - gap_penalty;
				float gap_extend = std::max(matrix_gap[ii][jj - 1], matrix_gap[ii - 1][jj]) - gap_extension;
				matrix_gap[ii - 1][jj - 1] = std::max(gap_open, gap_extend);
			}
		}    
	}
	
	// allow for gap at the beginning
	unsigned j_index=0;
	float max_score = matrix[0][0]; 
	for (unsigned i = 0; i <= len2; ++i)
		if (matrix[0][i] >= max_score)
		{
			max_score = matrix[0][i];
			j_index = i;
		}

	unsigned i_index=0; 
	for (unsigned i = 0; i <= len1; ++i)
		if (matrix[i][0] >= max_score)
		{
			max_score = matrix[i][0];
			i_index = i;
			j_index = 0;
		}


	// the starting sequence element is (i_index,j_index)
	unsigned ii = 0U, jj = 0U;
	if (i_index == 0)
	{
		if (j_index)
		{
			jj=j_index;
			pos1.push_back(t_gap(0,  j_index));
		}
	}
	else
	{
		ii=i_index;   
		pos2.push_back(t_gap(0,  i_index));
	}

	unsigned gap_len1 = 0, gap_len2 = 0;
	unsigned gap_start1 = 0, gap_start2 = 0;

	while (ii <= len1 && jj <= len2)
	{
		if (matrix[ii+1][jj+1] >= matrix[ii][jj+1] &&
			matrix[ii+1][jj+1] >= matrix[ii+1][jj] )
		{
			if (gap_len1)
			{
				pos1.push_back(t_gap(gap_start1, gap_len1));
				gap_start1 = gap_len1 = 0;
			}
			else if (gap_len2)
			{
				pos2.push_back(t_gap(gap_start2, gap_len2));
				gap_start2 = gap_len2 = 0;
			}
			++ii;
			++jj;
		}
		else
		{
			if (matrix[ii][jj+1] > matrix[ii+1][jj])	   // right > down
			{
				if (!gap_len1)
					gap_start1 = ii + 1;
				++gap_len1;
				++jj;

			}
			else
			{
				if (!gap_len2)
					gap_start2 = jj + 1;
				++gap_len2;
				++ii;
			}
		}
	}
	if (gap_len1)
	{
		pos1.push_back(t_gap(gap_start1, gap_len1));
		gap_start1 = gap_len1 = 0;
	}
	else if (gap_len2)
	{
		pos2.push_back(t_gap(gap_start2, gap_len2));
		gap_start2 = gap_len2 = 0;
	}

	// pad out the ends of sequences where necessary     

	if (ii != len1 + 1)
		pos2.push_back(t_gap(len2, len1 + 1 - ii));
	if (jj != len2 + 1)
		pos1.push_back(t_gap(len1, len2 + 1 - jj));

	//dump matrix
	/**
	std::cerr << "\t";
	for (unsigned i = 0; i < len1; ++i)
		std::cerr << "\t" << i;
	std::cerr << "\n";
	std::cerr << "\t";
	for (unsigned i = 0; i < len1; ++i)
		std::cerr << "\t" << *(first1 + i);
	std::cerr << "\n";
	for (unsigned j = 0; j <= len2; ++j)
	{
		std::cerr << j;
		if (j < len2)
			std::cerr << "\t" << *(first2 + j);
		else
			std::cerr << "\t ";
		for (unsigned i = 0; i <= len1; ++i)
		{
			std::cerr << "\t"  << matrix[i][j];
		}
		std::cerr << "\n";
	}
	/**/
}


